package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.BodyType;
import org.jbox2d.dynamics.World;
import playn.core.PlayN;

import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroMap;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

import java.util.HashMap;

/**
 * This class implements a force-directed algorithm .
 */
public class ForceSystem {
	
	/** Main JBox2d object. */
	public World	world;
	
	public MetroMap metroMap;

	/**
	 * Bodies in the JBox2D world that represent stations. The index is equal to
	 * the ID of the MetroStation, which is stored in the userData.
	 */
	public Body[]	stationBodies;

	/**
	 * Bodies in the JBox2D world that represent Bézier control points. The
	 * index is equal to the ID of the corresponding MetroEdge, which is stored
	 * in the userData.
	 */
	public Body[]	controlPointBodies;

	/**
	 * The scale factor between jbox and the model. model = modelScale * body
	 */
	public float	modelScale;

	/** The last update. */
	public double	lastUpdate = PlayN.currentTime();

	/** The subsystems. */
	public HashMap<String, ForceSubsystem> subsystems = new HashMap<String, ForceSubsystem>();
	
	/**
	 * Instantiates the JBox2D based force system.
	 * @param metroMap metro map which will be linked to the JBox2D world and
	 *            which will be modified after each call to update().
	 */
	public ForceSystem(MetroMap metroMap) {
		this.metroMap = metroMap;
		
		modelScale = calcModelScale(metroMap);

		// Create jbox2d world
		world = new World(new Vec2(0, 0));

		// Add bodies to world for stations
		createStationBodies(metroMap);
		// Add bodies for Bézier control points
		createControlPointBodies(metroMap);

		// Instantiate subsystems that apply forces for different constraints.
		subsystems.put("s0", new ForceS0(this));
		subsystems.put("s1", new ForceS1(this));
		subsystems.put("s2", new ForceS2(this));
		subsystems.put("s3", new ForceS3(this));
		subsystems.put("s4", new ForceS4(this));
		subsystems.put("s5", new ForceS5(this));
		subsystems.put("s6", new ForceS6(this));
		subsystems.put("s7", new ForceS7(this));
		
		lastUpdate = PlayN.currentTime();
	}

	/**
	 * Adds bodies to the world for the Bezier control points.
	 * @param metroMap the metro map which contains the list of stations.
	 */
	private void createControlPointBodies(MetroMap metroMap) {
		controlPointBodies = new Body[metroMap.edgeArray.length];

		// Loop through all edges
		for (MetroEdge edge : metroMap.edgeArray) {
			// Create body definition
			BodyDef edgeDef = new BodyDef();
			edgeDef.active = true;
			edgeDef.position = new Vec2(edge.cpX / modelScale, edge.cpY / modelScale);
			edgeDef.userData = edge;
			edgeDef.type = BodyType.DYNAMIC;
			//edgeDef.linearDamping = 0.0f;

			// Add new body to world;
			Body b = world.createBody(edgeDef);
			controlPointBodies[edge.id] = b;
		}
	}

	/**
	 * Adds bodies to the world that represent metro stations.
	 * @param metroMap the metro map which contains the list of stations.
	 */
	private void createStationBodies(MetroMap metroMap) {
		stationBodies = new Body[metroMap.stations.length];

		for (MetroStation m : metroMap.stations) {
			// Create metro station definition
			BodyDef stationDef = new BodyDef();
			stationDef.active = true;
			stationDef.position = new Vec2(m.x / modelScale, m.y / modelScale);
			stationDef.userData = m;
			stationDef.type = BodyType.DYNAMIC;
			//stationDef.linearDamping = 0.5f;

			// Add new body to world;
			Body b = world.createBody(stationDef);
			stationBodies[m.id] = b;
		}
	}

	/**
	 * Derives a model scale from a metro map, which can be used to scale the
	 * model into units which are appropriate for JBox2D.
	 * @param metroMap the metro map
	 * @return the calculated model scale
	 */
	private float calcModelScale(MetroMap metroMap) {
		// Derive model scale from the bounding box.
		float xmin = Float.POSITIVE_INFINITY;
		float xmax = Float.NEGATIVE_INFINITY;
		float ymin = Float.POSITIVE_INFINITY;
		float ymax = Float.NEGATIVE_INFINITY;
		for (MetroStation m : metroMap.stations) {
			xmin = Math.min(xmin, m.geoX);
			xmax = Math.max(xmax, m.geoX);
			ymin = Math.min(ymin, m.geoY);
			ymax = Math.max(ymax, m.geoY);
		}
		float dmax = Math.max(xmax - xmin, ymax - ymin);
		// jbox2d works great with objects between 0.1 and 100 meters.
		return (dmax != 0) ? (dmax / ForceConstants.BOUNDING_BOX_SIZE) : 1f;
	}

	/**
	 * Performs a step in the JBox2D world and updates the model.
	 * @param delta Time in milliseconds that has passed since last update.
	 */
	public void update(int delta) {
		for (int i = 0; i < 100; i++) {
			for (Body body : stationBodies) {
				body.setLinearVelocity(new Vec2());
			}
			for (Body body : controlPointBodies) {
				body.setLinearVelocity(new Vec2());
			}
			
			subsystems.get("s0").applyForces(delta);
			subsystems.get("s1").applyForces(delta);
//			subsystems.get("s2").applyForces(delta);
			subsystems.get("s3").applyForces(delta);
			subsystems.get("s4").applyForces(delta);
			subsystems.get("s5").applyForces(delta);
			subsystems.get("s6").applyForces(delta);
			
			float maxForceSq = Float.MIN_NORMAL;
			for (Body body : stationBodies) {
				float f = body.m_force.lengthSquared();
				if (maxForceSq < f)
					maxForceSq = f;
			}
			for (Body body : controlPointBodies) {
				float f = body.m_force.lengthSquared();
				if (maxForceSq < f)
					maxForceSq = f;
			}
			float maxForce = (float) Math.sqrt(maxForceSq);
			
			float dt = .005f / maxForce;
			
			for (Body body : stationBodies) {
				body.m_xf.p.addLocal(body.m_force.mul(dt));
				body.m_force.setZero();
			}
			
			for (Body body : controlPointBodies) {
				body.m_xf.p.addLocal(body.m_force.mul(dt));
				body.m_force.setZero();
			}
			
			//world.step(0.1f, 1, 1);
			updateModel();
		}
	}

	/**
	 * Sets the position of all model elements based on the position within the
	 * JBox2D world.
	 */
	private void updateModel() {
		for (Body body : stationBodies) {
			MetroStation m = (MetroStation) body.getUserData();
			if (m != null) {
				Vec2 pos = body.getPosition();
				m.x = pos.x * modelScale;
				m.y = pos.y * modelScale;
				m.bsX *= modelScale;
				m.bsY *= modelScale;
			}
		}

		for (Body body : controlPointBodies) {
			MetroEdge e = (MetroEdge) body.getUserData();
			if (e != null) {
				Vec2 pos = body.getPosition();
				e.cpX = pos.x * modelScale;
				e.cpY = pos.y * modelScale;
			}
		}
	}
}
